{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# MagPoseFactor\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/MagPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `MagPoseFactor<POSE>` is a templated factor designed to constrain the orientation component of a `Pose2` or `Pose3` variable using magnetometer readings. It's similar in purpose to `MagFactor1` but operates directly on pose types and explicitly handles an optional transformation between the body frame and the sensor frame.\n",
    "\n",
    "It assumes the magnetometer calibration parameters (scale, bias) and the local magnetic field direction are known."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Measurement Model\n",
    "\n",
    "The underlying model is the same as for `MagFactor`:\n",
    "$$ bM_{measured} = R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b $$ \n",
    "However, `MagPoseFactor` allows specifying an optional `body_P_sensor` transform. If provided, the factor internally adjusts the measurement and bias to be consistent with the body frame before calculating the error.\n",
    "\n",
    "Let:\n",
    "- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose` state.\n",
    "- $T_{bs}$ be the pose of the sensor in the body frame (`body_P_sensor`). If not given, $T_{bs}$ is identity.\n",
    "- $sM_{measured}$ be the raw measurement in the sensor frame.\n",
    "- $sB$ be the bias in the sensor frame.\n",
    "- $s$, $\\hat{d}_n$ be the known scale and nav-frame direction.\n",
    "\n",
    "The factor computes the predicted measurement *in the body frame*:\n",
    "$$ bM_{predicted} = R_{bs} [ R_{sn} (s \\cdot \\hat{d}_n) + sB ] $$ \n",
    "where $R_{sn} = R_{sb} R_{bn} = R_{bs}^T \\cdot R_{nb}^T$.\n",
    "\n",
    "Alternatively, and perhaps more clearly, it predicts the field in the sensor frame and compares it to the measurement:\n",
    "$$ sM_{predicted} = R_{sn} (s \\cdot \\hat{d}_n) + sB $$ \n",
    "$$ e = sM_{measured} - sM_{predicted} $$ \n",
    "The implementation transforms the measurement and bias to the body frame first for efficiency if `body_P_sensor` is provided.\n",
    "The error $e$ is calculated in the body frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Template Parameter**: `POSE` (must be `gtsam.Pose2` or `gtsam.Pose3`).\n",
    "- **Constructor**: `MagPoseFactor(poseKey, measured, scale, direction, bias, model, body_P_sensor=None)`\n",
    "  - `poseKey`: Key of the `Pose2` or `Pose3` variable.\n",
    "  - `measured`: Measured magnetic field vector (`Point2` or `Point3`) **in the sensor frame**.\n",
    "  - `scale`: Known scale factor.\n",
    "  - `direction`: Known magnetic field direction (`Point2` or `Point3`) **in the navigation frame**.\n",
    "  - `bias`: Known bias vector (`Point2` or `Point3`) **in the sensor frame**.\n",
    "  - `model`: Noise model (2D or 3D).\n",
    "  - `body_P_sensor`: Optional `Pose2` or `Pose3` describing the sensor's pose relative to the body frame.\n",
    "- **`evaluateError(nPb)`**: Calculates the error vector (2D or 3D) based on the current pose estimate `nPb`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example (Pose3)\n",
    "\n",
    "Using a magnetometer to help estimate a `Pose3` orientation, assuming known calibration and a sensor offset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created MagPoseFactor<Pose3>:\n",
      "  keys = { x0 }\n",
      "isotropic dim=3 sigma=50\n",
      "local field (nM): [35176.3235; 5025.18908; 35176.3235];\n",
      "measured field (bM): [28523.6117; 5040.18908; 40745.2206];\n",
      "magnetometer bias: [-10; 15; -5];\n",
      "\n",
      "Error at ground truth pose (should be zero): [0. 0. 0.]\n",
      "Error at test pose: [-3660.19475195     0.          2331.80122523]\n"
     ]
    }
   ],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "from gtsam.symbol_shorthand import X\n",
    "\n",
    "# --- Assumed Known Calibration & Field (NED Frame) ---\n",
    "n_direction_point = gtsam.Point3(0.7, 0.1, 0.7) # Example simplified direction in NED\n",
    "mag_scale = 50000.0 # nT\n",
    "mag_bias_sensor = gtsam.Point3(15.0, 10.0, -5.0) # Bias in sensor frame (nT)\n",
    "\n",
    "# --- Sensor Pose --- \n",
    "# Assume sensor is rotated 90 deg yaw right w.r.t body\n",
    "body_P_sensor = gtsam.Pose3(gtsam.Rot3.Yaw(np.deg2rad(90)), gtsam.Point3(0.1, 0, 0))\n",
    "sensor_P_body = body_P_sensor.inverse()\n",
    "\n",
    "# --- Simulation: Generate Measurement ---\n",
    "# Assume a ground truth body pose (e.g., 10 deg pitch up in NED)\n",
    "truth_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(10)), gtsam.Point3(1,2,3))\n",
    "truth_nRb = truth_nPb.rotation()\n",
    "truth_bRn = truth_nRb.inverse()\n",
    "\n",
    "# Calculate field in nav frame\n",
    "n_field_vector = mag_scale * (n_direction_point / np.linalg.norm(n_direction_point))\n",
    "\n",
    "# Calculate field in sensor frame \n",
    "truth_nRs = truth_nRb * body_P_sensor.rotation()\n",
    "truth_sRn = truth_nRs.inverse()\n",
    "s_field_ideal = truth_sRn.rotate(n_field_vector)\n",
    "\n",
    "# Calculate the measured value including bias (in sensor frame)\n",
    "s_measured = s_field_ideal + mag_bias_sensor\n",
    "\n",
    "# --- Factor Creation ---\n",
    "pose_key = X(0)\n",
    "\n",
    "# Noise model for the magnetometer measurement (nT)\n",
    "mag_noise_sigma = 50.0 # nT\n",
    "noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)\n",
    "\n",
    "# Create MagPoseFactor (providing body_P_sensor)\n",
    "# Note: measurement and bias are in SENSOR frame when body_P_sensor is specified\n",
    "mag_factor = gtsam.MagPoseFactorPose3(pose_key, s_measured, mag_scale, \n",
    "                                      n_direction_point, mag_bias_sensor, \n",
    "                                      noise_model, body_P_sensor)\n",
    "\n",
    "print(\"Created MagPoseFactor<Pose3>:\")\n",
    "mag_factor.print()\n",
    "\n",
    "# --- Evaluate Error ---\n",
    "# Evaluate at the ground truth pose (error should be zero)\n",
    "error_at_truth = mag_factor.evaluateError(truth_nPb)\n",
    "print(\"\\nError at ground truth pose (should be zero):\", error_at_truth)\n",
    "\n",
    "# Evaluate at a different pose (error should be non-zero)\n",
    "test_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(15)), gtsam.Point3(1,2,3))\n",
    "error_at_test = mag_factor.evaluateError(test_nPb)\n",
    "print(\"Error at test pose:\", error_at_test)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Important Considerations\n",
    "- **Frame Consistency**: Ensure `direction` is in the nav frame, while `measured` and `bias` are in the **sensor frame** when `body_P_sensor` is provided. If `body_P_sensor` is `None`, then `measured` and `bias` are assumed to be in the body frame.\n",
    "- **Units**: Maintain consistent units (e.g., nT) for scale, bias, measurement, and noise sigma."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [MagPoseFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/MagPoseFactor.h)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
